/*
 * MotorControl.cpp
 *
 *  Created on: Jan 18, 2014
 *      Author: ppks
 */

#include "common.h"
#include "MotorControl.h"
#include "FlightController.h"
#include "FlightController_Configuration.h"

/********************************************************************************/
/* Macro Definition                                                             */
/********************************************************************************/
#define PIDMIX(X, Y, Z)     (rcCommand[THROTTLE] + axisPID[ROLL] * X + axisPID[PITCH] * Y + YAW_DIRECTION * axisPID[YAW] * Z)
#define SERVODIR(n, b)      ((FC_Config.MISC.servoConf[n].rate & b) ? -1 : 1)

/******************************************************************************/
/* Local Variables Declaration                                                */
/******************************************************************************/
#if defined(MEGA)
    uint8_t PWM_PIN[8] = {3,5,6,2,7,8,9,10};      /* for a quad+: rear,right,left,front */
#endif

/******************************************************************************/
/* Local Functions Declaration                                                */
/******************************************************************************/


/******************************************************************************/
/* Function Code                                                              */
/******************************************************************************/
/*******************************************************************************
* Function Name: MotorControl_Init()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void MotorControl_Init(void)
{
    /* Set the correct drive mode for all output pins */
    for(uint8_t i = 0; i < NUMBER_OF_MOTORS; i++)
    {
        pinMode(PWM_PIN[i], OUTPUT);
    }

    /****************  Specific PWM Timers & Registers for the MEGA's    ******************/
#if defined(MEGA)
    #if (NUMBER_OF_MOTORS > 0)
        /* init 16bit timer 3 */
        TCCR3A |=  (1 << WGM31);    /* phase correct mode */
        TCCR3A &= ~(1 << WGM30);
        TCCR3B |=  (1 << WGM33);
        TCCR3B &= ~(1 << CS31);     /* no prescaler */
        ICR3   |= 0x3FFF;           /* TOP to 16383 */;

        TCCR3A |= _BV(COM3C1);      /* connect pin 3 to timer 3 channel C */
    #endif /* (NUMBER_OF_MOTORS > 0) */

    #if (NUMBER_OF_MOTORS > 1)
        TCCR3A |= _BV(COM3A1);      /* connect pin 5 to timer 3 channel A */
    #endif /* (NUMBER_OF_MOTORS > 1) */

    #if (NUMBER_OF_MOTORS > 2)
        // init 16bit timer 4
        TCCR4A |= (1<<WGM41);       /* phase correct mode */
        TCCR4A &= ~(1<<WGM40);
        TCCR4B |= (1<<WGM43);
        TCCR4B &= ~(1<<CS41);       /* no prescaler */
        ICR4   |= 0x3FFF;           /* TOP to 16383; */

        TCCR4A |= _BV(COM4A1);      /* connect pin 6 to timer 4 channel A */
    #endif /* (NUMBER_OF_MOTORS > 2) */

    #if (NUMBER_OF_MOTORS > 3)
        TCCR3A |= _BV(COM3B1);      /* connect pin 2 to timer 3 channel B */
    #endif /* (NUMBER_OF_MOTORS > 3) */

    #if (NUMBER_OF_MOTORS > 4)
        TCCR4A |= _BV(COM4B1);      /* connect pin 7 to timer 4 channel B */
        TCCR4A |= _BV(COM4C1);      /* connect pin 8 to timer 4 channel C */
    #endif /* (NUMBER_OF_MOTORS > 4) */

    #if (NUMBER_OF_MOTORS > 6)
        /* timer 2 is a 8bit timer so we cant change its range */
        TCCR2A |= _BV(COM2B1);      /* connect pin 9 to timer 2 channel B */
        TCCR2A |= _BV(COM2A1);      /* connect pin 10 to timer 2 channel A */
    #endif /* (NUMBER_OF_MOTORS > 6) */
#endif /* defined(MEGA) */

#if defined(ESC_CALIB_CANNOT_FLY)
      /********  special version of MultiWii to calibrate all attached ESCs ************/
      MotorControl_WriteToAllMotors(ESC_CALIB_HIGH);
      Alarms_BlinkLED(2,20, 2);
      delay(4000);
      MotorControl_WriteToAllMotors(ESC_CALIB_LOW);
      Alarms_BlinkLED(3,20, 2);
      while (1)
      {
          delay(5000);
          Alarms_BlinkLED(4,20, 2);
      #if defined(BUZZER)
          Alarms_SetAlarmLevel(ALARM_EVENT_CONFIRMATION, ALARM_LEVEL_2);
      #endif /* defined(BUZZER) */
      }
      exit; /* statement never reached */
#endif /* defined(ESC_CALIB_CANNOT_FLY) */

    /* Write minimal data to all motor control outputs at the init stage */
    MotorControl_WriteToAllMotors(MOTOR_MINCOMMAND);

    /* Wait 300 ms */
    delay(300);
}

/*******************************************************************************
* Function Name: MotorControl_WriteData()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void MotorControl_WriteData(void)
{
#if defined(MEGA)
    /* Multiply range of the command by 8 (for timer 3 & 4 for mega):
     * [1000:2000] => [8000:16000] */
    #if NUMBER_OF_MOTORS > 0
        OCR3C = motorControlData[0] << 3;   /* Motor 1 - pin 3 */
    #endif /* NUMBER_OF_MOTORS > 0 */

    #if NUMBER_OF_MOTORS > 1
        OCR3A = motorControlData[1] << 3;   /* Motor 2 - pin 5 */
    #endif /* NUMBER_OF_MOTORS > 1 */

    #if NUMBER_OF_MOTORS > 2
        OCR4A = motorControlData[2] << 3;   /* Motor 3 - pin 6 */
    #endif /* NUMBER_OF_MOTORS > 2 */

    #if NUMBER_OF_MOTORS > 3
        OCR3B = motorControlData[3] << 3;   /* Motor 4 - pin 2 */
    #endif /* NUMBER_OF_MOTORS > 3 */

    #if NUMBER_OF_MOTORS > 4
        OCR4B = motorControlData[4] << 3;   /* Motor 5 - pin 7 */
        OCR4C = motorControlData[5] << 3;   /* Motor 6 - pin 8 */
    #endif /* NUMBER_OF_MOTORS > 4 */

    #if NUMBER_OF_MOTORS > 6
        OCR2B = motorControlData[6] >> 3;   /* Motor 7 - pin 9 */
        OCR2A = motorControlData[7] >> 3;   /* Motor 8 - pin 10 */
    #endif /* NUMBER_OF_MOTORS > 6 */

#endif /* defined(MEGA) */
}

/*******************************************************************************
* Function Name: MotorControl_WriteToAllMotors()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void MotorControl_WriteToAllMotors(int16_t mc)
{
    /* Update the motor control data values */
    for (uint8_t i = 0; i < NUMBER_OF_MOTORS; i++)
    {
        motorControlData[i] = mc;
    }

    /* Write data to all motors outputs */
    MotorControl_WriteData();
}

/*******************************************************************************
* Function Name: MotorControl_MixTable()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void MotorControl_MixTable(void)
{
    int16_t maxMotor;
    uint8_t i;

#if defined( MULTICOPTER_TYPE_QUAD_P )
    motorControlData[0] = PIDMIX( 0,+1,-1); /* REAR */
    motorControlData[1] = PIDMIX(-1, 0,+1); /* RIGHT */
    motorControlData[2] = PIDMIX(+1, 0,+1); /* LEFT */
    motorControlData[3] = PIDMIX( 0,-1,-1); /* FRONT */
#elif defined( MULTICOPTER_TYPE_QUAD_X )
    motorControlData[0] = PIDMIX(-1,+1,-1); /* REAR_R */
    motorControlData[1] = PIDMIX(-1,-1,+1); /* FRONT_R */
    motorControlData[2] = PIDMIX(+1,+1,+1); /* REAR_L */
    motorControlData[3] = PIDMIX(+1,-1,-1); /* FRONT_L */
#endif /* MULTICOPTER_TYPE_XXX */

    /* Normalize Motor Values */
    maxMotor=motorControlData[0];

    /* Find max value in all motor outputs */
    for(i = 1; i < NUMBER_OF_MOTORS; i++)
    {
        if (motorControlData[i] > maxMotor)
        {
            maxMotor=motorControlData[i];
        }
    }

    for(i=0; i< NUMBER_OF_MOTORS; i++)
    {
        if (maxMotor > MAXTHROTTLE)
        {
            /* this is a way to still have good gyro corrections if at least one motor reaches its max. */
            motorControlData[i] -= (maxMotor - MAXTHROTTLE);
        }

        motorControlData[i] = constrain(motorControlData[i], FC_Config.MISC.minthrottle, MAXTHROTTLE);

        if ((rcData[THROTTLE] < RC_MINCHECK) && !globalFlags.BARO_MODE)
        {
        #ifndef MOTOR_STOP
            motorControlData[i] = FC_Config.MISC.minthrottle;
        #else
            motorControlData[i] = MOTOR_MINCOMMAND;
        #endif
        }
        /* If motor is not armed - then write minimal value */
        if (!globalFlags.ARMED)
        {
            motorControlData[i] = MOTOR_MINCOMMAND;
        }
    }
}
